#ifndef __IMU_H__
#define __IMU_H__

#include "main.h"
#include "kalman.h"

typedef struct
{
    float n;
    float cnt;
    float sum;
    float average;
    float flag;
} IMU_Filiter_TypeDef;

typedef struct
{
    float last_0;
    float last_1;
    float last_2;
    float last_3;
    float last_4;
} IMU_Last_TypeDef;

typedef struct 
{
    int n;
    IMU_Last_TypeDef Last[7];  // 改为7个，因为C代码中使用了0-6索引
    IMU_Filiter_TypeDef IMU_Filiter[3];
    Kalman_Typedef IMU_Kalman[3];
    float delta_time;
    float gx, gy, gz;
    float ax, ay, az;
    
    // 移除了last_ez，因为新方法不再需要角速度误差积分
    
    float q0, q1, q2, q3;
    float pitch, roll, yaw;
    
    // 新增：融合后的角度，用于调试和监控
    float roll_fused;
    float pitch_fused; 
    float yaw_fused;
    
} IMU_TypeDef;

extern IMU_TypeDef IMU_Data;

// 函数声明
void IMU_Data_Init(IMU_TypeDef* imu, float n, float delta_time);
void IMU_Error_Cal(IMU_Filiter_TypeDef* m, float x);
float IMU_bessel_filter(IMU_Last_TypeDef* m, float now, float filter);
void IMU_update(IMU_TypeDef* m, float gx, float gy, float gz, float ax, float ay, float az);
void IMU_Cal(IMU_TypeDef* m);
void IMU_to_euler(IMU_TypeDef* m);
void IMU_exe();

// 新增函数声明
void IMU_Get_Angle_From_Acc(IMU_TypeDef* m, float ax, float ay, float az, float* roll_acc, float* pitch_acc);
void IMU_Get_Angle_From_Quaternion(IMU_TypeDef* m, float* roll_gyro, float* pitch_gyro, float* yaw_gyro);
void IMU_Euler_to_Quaternion(float roll, float pitch, float yaw, float* q0, float* q1, float* q2, float* q3);

#endif